/**
 * @file   height_pid.c
 *
 * @brief  PID functions to control the height of the blimp
 *
 * CSC 460 Mantis Cheng Fall 2007
 *
 * @author Scott Craig
 * @author Justin Tanner
 */

#include "height_pid.h"
#include <string.h>
#include "../uart.h"

#define MAX_RECENT_RANGE	5
#define MAX_DIFF_RANGE		20



/* global variables */

//int recieved = 0;
int recentRange[MAX_RECENT_RANGE];
uint8_t range = 0;
int temp_range = 0;
int i;
int recentRangeIndex = 0;

char buff[50];


/** The height of the blimp from 6 to 255 inches */
uint8_t height;


/* local variables */
/** The reciprocal of the velocity of the blimp */
static int16_t vel_recip = INT16_MAX; /* A very large number */

/** The number of ticks since the height changed */
static uint16_t vel_recip_count = 1;

/** The calculated PID value */
static int16_t pid_value = 0;

/** Count the number of times the height drastically changes in a row */
static uint8_t error_count = 0;


/**
 * @brief Initilize the variables used in the PID controller.
 *
 */
void height_pid_init(void)
{
    /* A very large number means small velocity */
    vel_recip = INT16_MAX;
    vel_recip_count = 1;
    pid_value = 0;
    error_count = 0;
    height = HEIGHT_SET_POINT;
}

/**
 * @brief Calculate a new drive signal based on the current and previous output signals.
 *
 * This controller actually only uses the proportional and derivative components.
 * Assuming the blimp is neutrally bouyant, the integral component is not needed.
 *
 * @return the PID value
 *
 */
int16_t height_pid(void)
{
    uint8_t new_height;
	uint8_t old_height;

    

	recentRange[recentRangeIndex] = read_distance();
			
	//sprintf(buffer, " recentRange:%d  read_distance:%d", recentRange[recentRangeIndex], read_distance());
	//uart_write((uint8_t*)buffer, strlen(buffer));

	int count =0;

	range = 0;
	temp_range = 0;
	
	recentRangeIndex++;
	recentRangeIndex %= MAX_RECENT_RANGE;

	for(i=0; i<MAX_RECENT_RANGE; i++)
	{
		temp_range += (int) recentRange[i];
		count++;

	}		
	range = temp_range / count;

	range = read_distance();
	

//	sprintf(buff, " range:%d  \r\n", range);
//	uart_write((uint8_t*)buff, strlen(buff));
			


	new_height = range;

    /*
     *  The sonar produces output in inches, without a fractional part.
     *  Because we sample so frequently, the difference between successive
     *  height measurements is almost always 0.
     *  To produce a usable measure of velocity requires doing more
     *  than merely taking the difference between succesive heights.
     *  Instead, a count is kept of the number of ticks that have elapsed
     *  since he height meaurement has changed. When the height changes,
     *  this count is copied into a variable representing the reciprocal
     *  of the velocity. Then in the PID calculation, the derivative
     *  constant is divided by the reciprocal of the velocity, rather
     *  than multiplied by a velocity.
     *
     *  Also, spurious sonar readings are ignored by waiting until the height
     *  has been different for several measurements.
     */
    
	//	sprintf (buffer, "height:%d \n\r", height);
	//uart_write((uint8_t*)buffer, strlen(buffer));
	
	if (new_height == height)
    {
        if (vel_recip_count < INT16_MAX)
        {
            vel_recip_count++;
        }
    }
    else
    {
        if (error_count > 2)
        {
            if (new_height > height)
            {
                vel_recip = vel_recip_count;
            }
            else
            {
                vel_recip = -vel_recip_count;
            }

            old_height = height;
			height = new_height;
            error_count = 0;
            vel_recip_count = 1;

            pid_value = HEIGHT_KP * (old_height - height) - HEIGHT_KD / vel_recip;
        }
        else
        {
            error_count++;
        }
    }

    return pid_value;
}



/**
 * @brief Scales the value given by the pid() function to produce motor duty cycle.
 *
 * If the calculated pid is small, the the motor duty is zero.
 * Otherwise, a boost is added to the motor duty, because the motors do not spin
 * for small Voltages.
 *
 * @param pid value from the pid() function
 * @return translated value (-255 to 255)
 *
 */
int16_t height_scale(int16_t pid)
{
    int16_t duty;

    if(pid < HEIGHT_PID_CUTOFF && pid > -HEIGHT_PID_CUTOFF)
    {
        duty = 0;
    }
    else if(pid > 0)
    {
         duty = pid - HEIGHT_PID_CUTOFF + HEIGHT_PID_MIN_DUTY;
    }
    else
    {
        duty = pid + HEIGHT_PID_CUTOFF - HEIGHT_PID_MIN_DUTY;
    }


    if (duty > 255)
    {
        duty = 255;
    }

    if (duty < -255)
    {
        duty = -255;
    }

    return duty;
}

void setheight(int joystick_height){

		height = joystick_height;

}

uint8_t getHeight()
{
	return range;
}

